import sys
import termios
import tty
import rclpy
from rclpy.node import Node
from std_msgs.msg import Float64MultiArray

class TeleopKeyboardNode(Node):
    def __init__(self):
        super().__init__('teleop_keyboard')
        self.publisher_ = self.create_publisher(Float64MultiArray, '/forward_position_controller/commands', 10)
        # 关节初始值，可按需修改
        self.joint_positions = [0.0, 0.0, 0.0, 0.0, 0.0]  

    def get_key(self):
        fd = sys.stdin.fileno()
        old_settings = termios.tcgetattr(fd)
        try:
            tty.setraw(sys.stdin.fileno())
            key = sys.stdin.read(1)
        finally:
            termios.tcsetattr(fd, termios.TCSADRAIN, old_settings)
        return key

    def run_teleop(self):
        self.get_logger().info('使用 W/S 控制第一个关节，Q 退出。')
        while rclpy.ok():
            key = self.get_key()
            if key == 'w':
                self.joint_positions[0] += 0.1
            elif key == 's':
                self.joint_positions[0] -= 0.1
            elif key == 'q':
                break
            msg = Float64MultiArray()
            msg.data = self.joint_positions
            self.publisher_.publish(msg)

def main(args=None):
    rclpy.init(args=args)
    node = TeleopKeyboardNode()
    node.run_teleop()
    node.destroy_node()
    rclpy.shutdown()
